/**
 * @FilePath     : \simulations\include\agvWorld\node.h
 * @Description  : 定义了Node和NodeBase类的接口，描述了节点的属性、功能和操作，包括节点的类型、二维码处理、旋转等。 
 *                 用于AGV系统的地图和节点管理。  
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : error: error: git config user.name & please set dead value or install git && error: git config user.email & please set dead value or install git & please set dead value or install git
 * @LastEditTime : 2024-12-19 15:35:24
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once
#include <stdio.h>
#include "geometry.h"
#include "tools.h"
#include <vector>
#include <mutex>
#include "tinyxml2.h"
#include <cstring> // 用于strcpy
using namespace tinyxml2;

// Node type macros
#define NORMAL_NODE 0       // Whether it is normal node add by yxj
#define MARK_NODE BIT(0)    // Whether it has a mark
#define CHARGER_NODE BIT(1) // Whether it is a charger station
// #define COM_NODE BIT(2)

// The max. number of node is 0xFF00 (65280)
#define MAX_NODE_ID ((USHORT)0xFF00)
#define MAX_ZONE_NAME_BYTES 100 // 最大字节数
namespace xml_tool
{
    std::string FindMapId(XMLElement *root);
    XMLElement *FindMapData(XMLElement *root, const std::string &target_map_id);
}

struct QrOffsetData {
    float qr_offset_x;
    float qr_offset_y;
    float qr_offset_angle;
    QrOffsetData() : qr_offset_x(0.0f), qr_offset_y(0.0f), qr_offset_angle(0.0f) {}
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "Node".
class Node : public Point 
{
 public:
    // The constructor
    Node(unsigned short id, const Point& pt, unsigned short type = 0, float heading = 0.0f);

    // The default constructor
    Node();

    // Get qrCodeId
    int GetQrId() const { return qr_code_id_; };

    // Check if a mark is available at this node
    bool IsMarkNode() const;

    // Check if this node is a charger station
    bool IsChargerNode() const;

    // BOOL IsComNode();

    //  Check if this node can spin
    bool EnableSpin() const;

    //  Check if this node can rotate to node's angle.
    bool EnableNodeAngle() const;

    //  Check if this node can rotate to pick's angle.
    bool EnablePickAngle() const;

    //  Check if this node can rotate to drop's angle.
    bool EnableDropAngle() const;

    //  Check if this node can rotate to find qr code and load
    bool EnableLoadCentering() const;

    //  Check if this node can table turnning.
    bool EnableTableTunning() const;

    //  Check if this node can mark code.
    bool EnableMarkCode() const;

    // 前端配置16比特位 特殊使用
    uint16_t Bits() const;

    // 通用字段1和字段2
    void Words(int& word1, int& word2);

    // 节点自旋使用的避障方案
    int GetDetectPlan() const;

    // 获取执行节点动作前，旋转到制定角度的值
    void GetActionHead(float& head_pick, float& head_drop);

    // 获取跨楼层相关节点参数
    void GetCrossFloorProperty(std::string& zone_tag, bool& switch_point);

    // overloaded operator "=="
    bool operator==(const Node& nd) const;

    // overloaded operator "!="
    bool operator!=(const Node& nd) const;

    // overloaded operator "=="
    bool operator==(unsigned short node_id) const;

    // overloaded operator "!="
    bool operator!=(unsigned short node_id) const;

    // Create the node data from a text file
    // BOOL Create();
  
 public:
    unsigned short id_;                     // Node ID number
    unsigned short type_;                   // Node type
    float heading_;                         // Node heading
    int qr_code_id_;                        // QR code ID number
    float chk_mark_dist_;                   // Distance to check for a mark
    float chk_mark_vel_;                    // Velocity to check for a mark
    float max_spin_value_;                  // Max spin value when node spin
    float spin_acc_value_;                  // Max spin Acceleration  when node spin
    float spin_dcc_value_;                  // Max spin deceleration  when node spin
    float max_spin_load_;                   // Max spin value when node spin at load
    float spin_acc_load_;                   // Max spin Acceleration  when node spin at load
    float spin_dcc_load_;                   // Max spin deceleration  when node spin at load
    int detect_plan_;                       // Node spin obstacle detection plan
    bool enable_spin_;                      // Enable node spin
    bool enable_node_angle_;                // Enable node rotate to node's angle
    bool enable_pick_angle_;                // Enable node rotate to pick's angle
    bool enable_drop_angle_;                // Enable node rotate to drop's angle
    bool enable_load_centering_;            // Enable node rotate to find qr code and load
    bool enable_table_tunning_;             // Enable node table turnning
    bool enable_mark_code_;                 // Enable node mark code
    uint16_t bits_;                         // Front-end configuration
    QrOffsetData qr_offset_;                // QR code offset
    int word1_;                             // Common field 1
    int word2_;                             // Common field 2
    char zone_tag_[100];                    // Zone tag
    bool switch_point_;                     // Switch point
    float head_pick_;                       // Execute node action before rotate to pick's angle
    float head_drop_;                       // Execute node action before rotate to drop's angle

 private:
    void InitValue();
};

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "NodeBase".
class NodeBase 
{
public:
    // The default constructor
    NodeBase();

    // The destructor
    virtual ~NodeBase();

    // CNode GetNodeByQrCode(int nQrCode);
    // find the nearest node by x,y
    bool FindNearNodeByXy(const Posture& pst, Node& node, float threshold_dis);

    // Get the specified node object
    Node* GetNode(unsigned short id);

    // Get the pointer to the node data base object
    NodeBase& GetNodeBaseObject();

    // Create the node base from a text file
    bool Create(const std::string& map_path);

    // Check if the code is a valid node ID
    bool IsNode(unsigned short code);

private:
    void Clear();

    // Get the X coordinate of the left-most point
    float LeftMost() const;

    // Get the Y coordinate of the top-most point
    float TopMost() const;

    // Get the X coordinate of the right-most point
    float RightMost() const;

    // Get the Y coordinate of the bottom-most point
    float BottomMost() const;

    // Get the width of the map area
    float Width() const;

    // Get the height of the map area
    float Height() const;

    // Get the coordinates of the center point
    Point Center() const;

    // Add a node to the nodes data base
    short AddNode(Node& nd);

    // Delete a node from the nodes data base
    short RemoveNode(unsigned short id);

    // Modify the ID of a node
    short ModifyNodeID(unsigned short old_id, unsigned short new_id);

    // Modify the type of a node
    bool ModifyNodeType(unsigned short id, unsigned short new_type);

    // Modify the extended type of a node
    // BOOL ModifyNodeExtType(USHORT uId, SHORT uNewExtType);

    // Modify the location of a node
    bool ModifyNodePoint(unsigned short id, float x, float y);

protected:
    std::string map_id_;
public:
    unsigned short count_;
    Node* nodes_;
    std::mutex node_mutex_; // std::lock_guard<std::mutex> locker(m_ndMutex);
};
